//
// Created by 29245 on 2025/8/26.
//

#include "handle.h"
#include "Servo.h"
#include "pca9685.h"
#include "delay.h"
#include <math.h>
#include <stdio.h>
#include "spider_leg_kinematics.h"


// 腿部舵机ID映射数组：[body_servo_id, limb_servo_id, foot_servo_id]
static const uint8_t leg_servo_map[6][3] = {
    {1, 7, 13},   // 腿1: 底盘1, 腿关节7, 脚关节13
    {2, 8, 14},   // 腿2: 底盘2, 腿关节8, 脚关节14
    {3, 9, 15},   // 腿3: 底盘3, 腿关节9, 脚关节15
    {4, 10, 16},  // 腿4: 底盘4, 腿关节10, 脚关节16
    {5, 11, 17},  // 腿5: 底盘5, 腿关节11, 脚关节17
    {6, 12, 18}   // 腿6: 底盘6, 腿关节12, 脚关节18
};

//第一个
 // 舵机误差修正值数组，索引0对应舵机1，索引17对应舵机18
 static const int8_t servo_trim[18] = {
     0, 5, -5, 5, -5, -5,     // 舵机1-6 (底盘)
     -8, -3, -2, 3, 1, -2,     // 舵机7-12 (腿关节)
      5, 5, -10, -3, 5, 0      // 舵机13-18 (脚关节)
 };
//
// //第二个
// // 舵机误差修正值数组，索引0对应舵机1，索引17对应舵机18
// static const int8_t servo_trim[18] = {
//     1, 5, 4, 2, 2, 2,     // 舵机1-6 (底盘)
//     -3, 5, -5, -1, 2, -8,     // 舵机7-12 (腿关节)
//      -2, 5, 0, -5, 0, 5      // 舵机13-18 (脚关节)
// };
/**
 * 控制18个舵机的角度
 * @param servoId 舵机编号 (1-18)
 * @param angle 角度值 (-90到90度)
 */
void Servo(uint8_t servoId, int16_t angle) {
    // 限制角度在有效范围内
    if (angle > 90) {
        angle = 90;
    } else if (angle < -90) {
        angle = -90;
    }

    // 转换角度范围从-90~90到0~180度来适应舵机
    uint16_t servo_angle = (uint16_t)(angle + 90);

    // 应用误差修正值
    if (servoId >= 1 && servoId <= 18) {
        int16_t corrected_angle = (int16_t)servo_angle + servo_trim[servoId - 1];
        // 确保修正后的角度仍在有效范围内
        if (corrected_angle < 0) corrected_angle = 0;
        if (corrected_angle > 180) corrected_angle = 180;
        servo_angle = (uint16_t)corrected_angle;
    }

    // 1-11号舵机，对应setAngle()的0-10通道
    if (servoId >= 1 && servoId <= 11) {
        setAngle(servoId - 1, servo_angle);
    }
    // 12号舵机对应TIM_CHANNEL_4
    else if (servoId == 12) {
        Servo_SetAngle(TIM_CHANNEL_4, servo_angle);
    }
    // 13-17号舵机，对应setAngle()的11-15通道
    else if (servoId >= 13 && servoId <= 17) {
        setAngle(servoId - 2, servo_angle);  // 13-17号舵机对应11-15通道(减2)
    }
    // 18号舵机对应TIM_CHANNEL_3
    else if (servoId == 18) {
        Servo_SetAngle(TIM_CHANNEL_3, servo_angle);
    }
}


/**
 * 控制底盘舵机组(1-6)的角度
 * 髋关节
 * @param angle 角度值 (-90到90度)
 */
void body_handle(uint16_t angle) {
    Servo(1, angle);
    Servo(2, angle);
    Servo(3, angle);
    Servo(4, angle);
    Servo(5, angle);
    Servo(6, angle);
}

/**
 * 控制腿关节舵机组(7-12)的角度
 * 膝关节
 * @param angle 角度值 (-90到90度)
 */
void limb_handle(uint16_t angle) {
    Servo(7, angle);
    Servo(8, angle);
    Servo(9, angle);
    Servo(10, angle);
    Servo(11, angle);
    Servo(12, angle);
}

/**
 * 控制脚关节舵机组(13-18)的角度
 * 踝关节
 * @param angle 角度值 (-90到90度)
 */
void foot_handle(uint16_t angle) {
    Servo(13, angle);
    Servo(14, angle);
    Servo(15, angle);
    Servo(16, angle);
    Servo(17, angle);
    Servo(18, angle);
}


/**
 * 控制指定腿的角度
 * @param legId 腿编号 (1-6)
 * @param body 髋关节角度 (-45到45度)
 * @param limb 膝关节角度 (-75到75度)
 * @param foot 踝关节角度 (-60到60度)
 */
void leg_handle(uint8_t legId, int16_t body, int16_t limb, int16_t foot) {
    if (legId >= 1 && legId <= 6) {
        // 关节角度限制范围
        const int16_t body_min = -45, body_max = 45;    // 髋关节角度限制
        const int16_t limb_min = -45, limb_max = 75;    // 膝关节角度限制  
        const int16_t foot_min = -60, foot_max = 60;    // 踝关节角度限制
        
        // 角度限幅处理
        if (body < body_min) body = body_min;
        else if (body > body_max) body = body_max;
        
        if (limb < limb_min) limb = limb_min;
        else if (limb > limb_max) limb = limb_max;
        
        if (foot < foot_min) foot = foot_min;
        else if (foot > foot_max) foot = foot_max;

        // 根据腿编号查找对应的舵机ID并控制
        Servo(leg_servo_map[legId-1][0], body);              // 底盘舵机
        Servo(leg_servo_map[legId-1][1], limb);              // 腿关节舵机
        Servo(leg_servo_map[legId-1][2], foot);              // 脚关节舵机
        //printf("腿%d: 底盘=%d° 腿关节=%d° 脚关节=%d°\n",
        //   legId, body, limb, foot);
    }
}


/**
 * 标定基础位置
 * @param body 髋关节角度 (-90到90度)
 * @param limb 膝关节角度 (-90到90度)
 * @param foot 踝关节角度 (-90到90度)
 */
void stand_mark(void) {
    // 控制底盘舵机组(1-6)到90度
    body_handle(0);

    // 控制腿关节舵机组(7-12)到120度
    limb_handle(0);

    // 控制脚关节舵机组(13-18)到170度
    foot_handle(0);
}

/**
 * 站立动作函数
 * 第一组(底盘): 0度
 * 第二组(腿关节): 30度
 * 第三组(脚关节): 40度
 */
void stand_action(void) {
    // // 控制底盘舵机组(1-6)到90度
    // body_handle(90);
    //
    // // 控制腿关节舵机组(7-12)到120度
    // limb_handle(130);
    //
    // // 控制脚关节舵机组(13-18)到170度
    // foot_handle(165);
    leg_handle(1, 0, 30, 40);
    leg_handle(2, 0, 30, 40);
    leg_handle(3, 0, 30, 40);
    leg_handle(4, 0, 30, 40);
    leg_handle(5, 0, 30, 40);
    leg_handle(6, 0, 30, 40);
}



/**
 * 使用逆解算控制指定腿的脚尖位置
 * @param legId 腿编号 (1-6)
 * @param target_x 目标X坐标 (mm)
 * @param target_y 目标Y坐标 (mm)
 * @param target_z 目标Z坐标 (mm)
 */
void leg_move_to_position(uint8_t legId, float target_x, float target_y, float target_z) {
    if (legId < 1 || legId > 6) {
        //printf("错误：腿编号无效\n");
        return;
    }

    // 构造目标坐标
    D4Coord target_coord = {target_x, target_y, target_z};
    ServoAngles servo_ang;

    // 进行逆解算
    LegStatus status = Leg_InverseKinematics(target_coord, &servo_ang);

    if (status == LEG_OK) {
        // 使用解算出的角度控制舵机
        leg_handle(legId,
                  (int16_t)servo_ang.body,   // 髋关节角度
                  (int16_t)servo_ang.limb,   // 膝关节角度
                  (int16_t)servo_ang.foot);  // 踝关节角度
        //printf("腿%d移动到位置: x=%.2f, y=%.2f, z=%.2f\n", legId, target_x, target_y, target_z);
    } else {
        //printf("腿%d无法移动到位置: x=%.2f, y=%.2f, z=%.2f (超出工作空间)\n",
        //       legId, target_x, target_y, target_z);
    }
}
/**
 * 使用全局坐标系控制指定腿的脚尖位置
 * @param legId 腿编号 (1-6)
 * @param global_x 全局目标X坐标 (mm)
 * @param global_y 全局目标Y坐标 (mm)
 * @param global_z 全局目标Z坐标 (mm)
 */
void leg_move_to_global_position(uint8_t legId, float global_x, float global_y, float global_z) {
    if (legId < 1 || legId > 6) {
        return;
    }

    // 构造全局目标坐标
    D4Coord global_coord = {global_x, global_y, global_z};
    ServoAngles servo_ang;

    // 进行全局逆解算
    LegStatus status = Leg_InverseKinematicsGlobal(legId-1, global_coord, &servo_ang);

    if (status == LEG_OK) {
        // 使用解算出的角度控制舵机
        leg_handle(legId,
                  (int16_t)servo_ang.body,   // 髋关节角度
                  (int16_t)servo_ang.limb,   // 膝关节角度
                  (int16_t)servo_ang.foot);  // 踝关节角度
        //("腿%d: [正解]移动到全局位置: x=%.2f, y=%.2f, z=%.2f\n", legId, global_x, global_y, global_z);
        //printf("腿%d: [逆解]底盘=%d° 腿关节=%d° 脚关节=%d°\n",
        //   legId, (int16_t)servo_ang.body, (int16_t)servo_ang.limb, (int16_t)servo_ang.foot);
    } else {
        //printf("腿%d无法移动到全局位置: x=%.2f, y=%.2f, z=%.2f\n",
        //       legId, global_x, global_y, global_z);
    }
}
